#include <avr/io.h>
#include <stdlib.h>
#include <limits.h>
#include <string.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include "packet.h"
#include "motor.h"
#include "sonar.h"
#include "common.h"
#include "radio.h"
#include "uart.h"
#include "os.h"

#define T_TEST 1
#define T_MOTOR 2

void send_sensor_data();
void send_motor_data();


const unsigned char PPP[] = {T_TEST, 10, T_TEST, 10, T_TEST, 10, T_TEST, 10, T_TEST, 10, T_MOTOR, 2};
const unsigned int PT = 6;

const unsigned short send_address = BASE_ADDRESS;
const unsigned short recv_address = HOVER_ADDRESS;

uint8_t send_buffer[PAYLOAD_BYTES];

struct pid{
	float p, i, d;
};

void pid_init(struct pid *p){
	p->p = p->i = p->d = 0;
}

void pid_update(struct pid *p, float e){
	p->d = e - p->p;
	p->i += e;
	p->p = e;
}

/*#define YP (5.0 * 0.6)
#define YI (2.0 * (YP) / 20.0)
#define YD (YP * 5.0 / 2.0)*/
#define YP 7.0
#define YI (2.0 * (YP) / 20.0)
#define YD (YP * 5.0 / 2.0)

#define WALL_DIST 30.0
#define RP (30.0)
#define RI (2.0 * (RP) / 23.0)
#define RD (RP * 23.0 / 8.0)

#define FRONT_DIST 20.0
#define FP (1.0)
#define FI (0.0)
#define FD (10.0)

void pid_yaw(struct pid *y){
	int el, er;
	el = er = INT_MAX;

	/* Ignore readings over 100 in away*/
	if(read_distance(0) < 100 && read_distance(1) < 100)
		er = ((int)read_distance(0) - (int)read_distance(1));
	if(read_distance(2) < 100 && read_distance(3) < 100)
		el = ((int)read_distance(3) - (int)read_distance(2));

	/* Follow wall with least error */
	int yerr = abs(el) > abs(er) ? er : el;
	yerr = el;
	
	if(yerr != INT_MAX){
		pid_update(y, yerr);
	}
}

void motor_task(void){
	struct pid y;
	struct pid r;
	pid_init(&y);
	pid_init(&r);
	for(;;){
		pid_yaw(&y);
		/* Update translational PID with distance from center of hallway */
		pid_update(&r, WALL_DIST - ((int)read_distance(2) + (int)read_distance(3))/2);

		/* Calculate motor speeds */
		int8_t yaw   = (int8_t)(y.p*YP + y.i*YI + y.d*YD);
		int8_t trans = (int8_t)(r.p*RP + r.i*RI + r.d*RD);

		/* Thrust forwards when we have a relatively correct yaw */
		thrust = y.p > 5 ? 0 : 70;

		/* Actually set motors */
		motor_set_speed(0, trans);
		motor_set_speed(2, trans);
		motor_set_speed(1, -yaw + thrust);
		motor_set_speed(3, yaw + thrust);

		/* Yield to other tasks */
		Task_Next();
	}
}

void sonar_task(void){
	int idx;
	for(;;){
		for(idx = 0; idx < 5; idx++){
			PORTD ^= _BV(PORTD5);
			sonar_pulse(idx);
			Task_Next();
		}
	}
}

void hello(){
	send_buffer[0] = 0;	
	strcpy((char *)&send_buffer[1], "Hello from Hover!");
	send_data(send_buffer);
	_delay_ms(100);
}


void send_sensor_data(){
	//memset(send_buffer,0,PAYLOAD_BYTES);
	send_buffer[0] = 1;	// sensor data
	int i;
	for(i=0; i<5; i++)	// fetch sonar distance
		send_buffer[i+1] = (uint8_t)read_distance(i);

		
	for(i=0; i<4; i++)
		send_buffer[7+i] = (uint8_t)motor_get_speed(i);
	

	send_data(send_buffer);
}

void send_motor_data(){
	memset(send_buffer,0,PAYLOAD_BYTES);
	send_buffer[0] = 2;	// motor data
//	int i;
//	for(i=0; i<5; i++)	// fetch sonar distance
//		send_buffer[i+1] = (uint8_t)read_distance(i);	
	send_data(send_buffer);
}


int main(){
	DISABLE_INTERRUPTS();
	DISABLE_PRESCALER();
	ENABLE_INTERRUPTS();
	DDRD = 0xff;

	radio_init();
	hello();

	if(!Task_Create(sonar_task, 0, PERIODIC, T_TEST))
		OS_Abort();
	if(!Task_Create(motor_task, 0, PERIODIC, T_MOTOR))
		OS_Abort();
	
	motor_init();
	DDRB = 0xff;
	DDRD = 0xff;
	sonar_init();
	DDRC= 0xff;

	return 0;
}

/**
 * receive radio packet interrupt
 **/
ISR(INT4_vect){
}

